package projects.defaultProject.nodes.nodeImplementations;

import java.awt.Color;
import java.awt.Graphics;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Vector;

import projects.defaultProject.nodes.timers.MessageTimer;
import projects.ufam.CustomGlobal;
import projects.ufam.filters.ParticleFilter;
import projects.ufam.nodes.messages.AodvRrepMessage;
import projects.ufam.nodes.messages.AodvRreqMessage;
import projects.ufam.nodes.messages.LocalizationMessage;
import projects.ufam.nodes.messages.RPEMessage;
import projects.ufam.nodes.messages.TrackingMsg;
import sinalgo.configuration.Configuration;
import sinalgo.configuration.WrongConfigurationException;
import sinalgo.gui.transformation.PositionTransformation;
import sinalgo.nodes.Node;
import sinalgo.nodes.NodeState;
import sinalgo.nodes.Position;
import sinalgo.nodes.messages.Inbox;
import sinalgo.nodes.messages.Message;
import sinalgo.runtime.Global;
import sinalgo.tools.statistics.GaussianDistribution;
import ema.Matrix;

public class LocalizableNode extends Node {
	public static int NUM_ROUNDS_TRACKING = 40;
	int trackingMsgRound = 0;
	ParticleFilter filter = new ParticleFilter(1000);// Conforme recomendado no
	// artigo do Éfren
	protected boolean f = false;

	private class RoutingEntry {
		public Node next = null;
		public int hops = 0;
		public Integer id = 0;
		public int time = 0;
	}

	public int roundRreq = 0;
	HashSet<Message> received = new HashSet<Message>();
	boolean sink;
	protected Position estimatedPosition;
	protected double residual;
	private Position recursionOrigin;
	HashMap<Integer, AodvRreqMessage> aodvRreqs = new HashMap<Integer, AodvRreqMessage>();
	HashMap<Integer, AodvRrepMessage> aodvRreps = new HashMap<Integer, AodvRrepMessage>();
	HashMap<Integer, ArrayList<TrackingMsg>> trackingMessages = new HashMap<Integer, ArrayList<TrackingMsg>>();
	protected ArrayList<TrackingMsg> cache = new ArrayList<TrackingMsg>();
	/**
	 * Tabela de roteamento: <Destino, Proximo Nó>
	 */
	protected HashMap<Node, RoutingEntry> routingTable = new HashMap<Node, RoutingEntry>();
	/**
	 * Distâncias para os vizinhos
	 */
	protected HashMap<Node, Double> neighbors = new HashMap<Node, Double>();

	protected HashMap<Node, Double> localizedNeighbors = new HashMap<Node, Double>();

	public LocalizableNode() {
		super();
		resetRouting();
	}

	public void resetRouting() {
		routingTable.clear();
		RoutingEntry re = new RoutingEntry();
		re.hops = 0;
		re.id = Integer.MAX_VALUE;
		re.next = this;
		routingTable.put(this, re);
	}

	public void onLocalized() {
	}

	/**
	 * Estimates distance between two nodes. This must be done with actual
	 * positions to emulate RSSI
	 * 
	 * @param node
	 *            node
	 * @return estimated distance (RSSI)
	 */
	public Double estimateDistance(Node node) {
		return estimateDistance(((LocalizableNode) node).getPosition());
	}

	public Double estimateDistance(Position p) {
		Double result = getPosition().distanceTo(p);
		result += GaussianDistribution.nextGaussian(CustomGlobal.noiseMean(),
				result * CustomGlobal.noiseVariance());
		return result;
	}

	protected void calculateResidual(LocalizableNode[] references,
			Double[] distances) {
		int n = references.length;
		residual = 0;
		for (int j = 0; j < n; j++) {
			double xr = references[j].getEstimatedPosition().xCoord;
			double yr = references[j].getEstimatedPosition().yCoord;
			double zr = references[j].getEstimatedPosition().zCoord;

			double x = this.getEstimatedPosition().xCoord;
			double y = this.getEstimatedPosition().yCoord;
			double z = this.getEstimatedPosition().zCoord;

			double d = this.getEstimatedPosition().distanceTo(
					references[j].getEstimatedPosition());
			residual += Math.pow(d - distances[j], 2);
		}
	}

	protected Position multilateration2d(LocalizableNode[] references,
			Double[] distances) {
		// Definindo a quantidade máxima de referências que serão utilizadas
		int n = references.length - 1;
		if (n > CustomGlobal.maxReferences())
			n = CustomGlobal.maxReferences();

		// Multilateracao
		double[][] A = new double[n][2];
		double[][] b = new double[n][1];

		for (int i = 0; i < n; i++) {
			A[i][0] = references[n].getEstimatedPosition().xCoord
					- references[i].getEstimatedPosition().xCoord;
			A[i][1] = references[n].getEstimatedPosition().yCoord
					- references[i].getEstimatedPosition().yCoord;

			b[i][0] = (Math.pow(distances[i], 2) - Math.pow(distances[n], 2))
					- (Math.pow(references[i].getEstimatedPosition().xCoord, 2) - Math
							.pow(references[n].getEstimatedPosition().xCoord, 2))
					- (Math.pow(references[i].getEstimatedPosition().yCoord, 2) - Math
							.pow(references[n].getEstimatedPosition().yCoord, 2));
		}
		A = Matrix.times(A, 2);
		double[][] p = Matrix.solve(A, b);
		return (new Position(p[0][0], p[1][0], 0));
	}

	protected Position multilateration2d(Position[] pos, Double[] distances) {
		// Definindo a quantidade máxima de referências que serão utilizadas
		int n = pos.length - 1;
		if (n > CustomGlobal.maxReferences())
			n = CustomGlobal.maxReferences();

		// Multilateracao
		double[][] A = new double[n][2];
		double[][] b = new double[n][1];

		for (int i = 0; i < n; i++) {
			A[i][0] = pos[n].xCoord - pos[i].xCoord;
			A[i][1] = pos[n].yCoord - pos[i].yCoord;

			b[i][0] = (Math.pow(distances[i], 2) - Math.pow(distances[n], 2))
					- (Math.pow(pos[i].xCoord, 2) - Math.pow(pos[n].xCoord, 2))
					- (Math.pow(pos[i].yCoord, 2) - Math.pow(pos[n].yCoord, 2));
		}
		A = Matrix.times(A, 2);
		double[][] p = Matrix.solve(A, b);
		return (new Position(p[0][0], p[1][0], 0));
	}

	protected Position multilateration3d(LocalizableNode[] references,
			Double[] distances) {
		// Definindo a quantidade máxima de referências que serão utilizadas
		int n = references.length - 1;
		if (n > CustomGlobal.maxReferences())
			n = CustomGlobal.maxReferences();

		// Multilateracao
		double[][] A = new double[n][3];
		double[][] b = new double[n][1];

		for (int i = 0; i < n; i++) {
			A[i][0] = references[n].getEstimatedPosition().xCoord
					- references[i].getEstimatedPosition().xCoord;
			A[i][1] = references[n].getEstimatedPosition().yCoord
					- references[i].getEstimatedPosition().yCoord;
			A[i][2] = references[n].getEstimatedPosition().zCoord
					- references[i].getEstimatedPosition().zCoord;

			b[i][0] = (Math.pow(distances[i], 2) - Math.pow(distances[n], 2))
					- (Math.pow(references[i].getEstimatedPosition().xCoord, 2) - Math
							.pow(references[n].getEstimatedPosition().xCoord, 2))
					- (Math.pow(references[i].getEstimatedPosition().yCoord, 2) - Math
							.pow(references[n].getEstimatedPosition().yCoord, 2))
					- (Math.pow(references[i].getEstimatedPosition().zCoord, 2) - Math
							.pow(references[n].getEstimatedPosition().zCoord, 2));
		}

		A = Matrix.times(A, 2);
		double[][] p = Matrix.solve(A, b);
		return (new Position(p[0][0], p[1][0], p[2][0]));
	}

	public HashMap<Node, Double> getNeighbors() {
		return neighbors;
	}

	public void setNeighbors(HashMap<Node, Double> neighbors) {
		this.neighbors = neighbors;
	}

	public Position getEstimatedPosition() {
		return CustomGlobal.getLocalizationAlgorithm() == null ? getPosition()
				: estimatedPosition;
	}

	public void setEstimatedPosition(Position estimatedPosition) {
		this.estimatedPosition = estimatedPosition;
	}

	public boolean isBeaconNode() {
		return (this.getPosition() == this.estimatedPosition);
	}

	public boolean isFreeNode() {
		return (this.getEstimatedPosition() == null);
	}

	public boolean isSettledNode() {
		return (!isBeaconNode() && !isFreeNode());
	}

	public boolean isReferenceNode() {
		return (isBeaconNode() || isSettledNode());
	}

	public double getResidual() {
		return residual;
	}

	@NodePopupMethod(menuText = "Make Beacon")
	public void makeBeacon() {
		this.estimatedPosition = this.getPosition();
		this.setEnergyLevel(1.0);
		LocalizationMessage message = new LocalizationMessage(this, 0);
		MessageTimer timer = new MessageTimer(message);
		timer.startRelative(1, this);
		onLocalized();
	}

	public void useRealPosition() {
		this.estimatedPosition = this.getPosition();
		this.setEnergyLevel(1.0);
		this.residual = 0;
		onLocalized();
	}

	@Override
	public void checkRequirements() throws WrongConfigurationException {
		// TODO Auto-generated method stub

	}

	public void startDensityControl() {

	}

	@Override
	public void preStep() {
		// Dá 50 rounds para os nós se localizarem
		// Sem isto fica injusto para a localização exata, que começa a rastrear
		// antes.
		if ((int) Global.currentTime == 150)
			startDensityControl();
		if (this == CustomGlobal.sink)
			return;
		if (this.isFreeNode() || this.getState() != NodeState.Enabled)
			return;
		if (!CustomGlobal.targetEnabled)
			return;
		// Usa posicao real para determinar se o alvo esta ao alcance
		double d = this.getPosition().distanceTo(
				CustomGlobal.target.getPosition());
		if (d <= CustomGlobal.sensorRange && Global.currentTime % 100 == 0) {
			TrackingMsg msg = new TrackingMsg(this);
			received.add(msg);
			// Distancia real - emulando potencia do sinal
			msg.setDistance(this.estimateDistance(CustomGlobal.target
					.getPosition()));
			msg.setHops(0);
			if (routingTable.containsKey(CustomGlobal.sink)
					&& Global.currentTime
							- routingTable.get(CustomGlobal.sink).time < CustomGlobal.aodvExpirationTime) {
				d = this.getEstimatedPosition().distanceTo(
						CustomGlobal.target.getPosition());
				send(msg, routingTable.get(CustomGlobal.sink).next);
				msg.msgSent();
				// Se não recebi depois de 100 rounds, mando de novo
			} else if (((int) Global.currentTime) - roundRreq > 100) {
				roundRreq = (int) Global.currentTime;
				AodvRreqMessage m = new AodvRreqMessage(this,
						CustomGlobal.sink, 0);
				this.broadcast(m);
				cache.add(msg);
			} else
				cache.add(msg);
			// else deixa a mensagem pra lá
		}
	}

	public void handleMessage(Message msg, Node sender) {
		if (msg instanceof AodvRreqMessage && getState() == NodeState.Enabled) {
			AodvRreqMessage m = (AodvRreqMessage) msg;
			// Se não for repetido
			if (!aodvRreqs.containsKey(m.getId())) {
				// guarda na tabela
				aodvRreqs.put(m.getId(), m);
				boolean add = false;
				if (!routingTable.containsKey(m.getSource())) {
					add = true;
				} else {
					if (Global.currentTime
							- routingTable.get(m.getSource()).time > CustomGlobal.aodvExpirationTime)
						add = true;
				}
				if (add) {
					RoutingEntry re = new RoutingEntry();
					re.hops = m.getHops();
					re.id = m.getId();
					re.next = sender;
					re.hops = m.getHops();
					re.time = (int) Global.currentTime;
					routingTable.put(m.getSource(), re);
					// Criar timer para remover esta entrada da tabela.
				}
				// Se Não tiver a rota ou ela for velha
				if (m.getDestination().ID != ID
						&& (!routingTable.containsKey(m.getDestination()) || Global.currentTime
								- routingTable.get(m.getDestination()).time > CustomGlobal.aodvExpirationTime)) {
					// Passa a rreq pra frente
					m = (AodvRreqMessage) m.clone();
					m.setHops(m.getHops() + 1);
					this.broadcast(m);
				} else {
					// Se tiver rota, manda a rota
					if (m.getHops() <= routingTable.get(m.getDestination()).hops
							|| m.getDestination() == this) {
						AodvRrepMessage rrep = new AodvRrepMessage(routingTable
								.get(m.getDestination()).next, m.getSource(),
								routingTable.get(m.getDestination()).hops);
						this.send(rrep, sender);
					}
				}
			}
		}
		// Se for uma rrep
		if (msg instanceof AodvRrepMessage && getState() == NodeState.Enabled) {
			AodvRrepMessage m = (AodvRrepMessage) msg;
			if (!aodvRreps.containsKey(m.getId())) {
				aodvRreps.put(m.getId(), m);
				// Criamos uma entrada para o remetente da mensagem se:
				// 1. Não temos uma entrada;
				// 2. A nova mensagem traz uma rota mais rápida; ou
				// 3. A entrada antiga é velha
				if (!routingTable.containsKey(m.getSrc())
						|| m.getHops() < routingTable.get(m.getSrc()).hops
						|| Global.currentTime
								- routingTable.get(m.getSrc()).time > CustomGlobal.aodvExpirationTime) {
					RoutingEntry re = new RoutingEntry();
					re.hops = m.getHops();
					re.id = m.getId();
					re.next = sender;
					re.time = (int) Global.currentTime;
					routingTable.put(m.getSrc(), re);
				}
				if (!routingTable.containsKey(CustomGlobal.sink)
						|| m.getHops() < routingTable.get(CustomGlobal.sink).hops
						|| Global.currentTime
								- routingTable.get(CustomGlobal.sink).time > CustomGlobal.aodvExpirationTime) {
					RoutingEntry re = new RoutingEntry();
					re.hops = m.getHops();
					re.id = m.getId();
					re.next = sender;
					re.time = (int) Global.currentTime;
					routingTable.put(CustomGlobal.sink, re);
				}
				for (TrackingMsg tm : cache) {
					this.send(tm, m.getSrc());
					TrackingMsg.msgSent();
				}
				cache.clear();
				if (routingTable.get(m.getDst()) != null) {
					if (routingTable.get(m.getSrc()).hops >= m.getHops()) {
						m = (AodvRrepMessage) m.clone();
						m.setHops(m.getHops() + 1);
						if (routingTable.get(m.getDst()).next != this) {
							MessageTimer mt = new MessageTimer(m, routingTable
									.get(m.getDst()).next);
							mt.startRelative(1, this);
						}
					}
				}
			}
		}
		if (msg instanceof TrackingMsg && getState() == NodeState.Enabled) {
			if (received.contains(msg))
				return;
			received.add(msg);
			if (this == CustomGlobal.sink) {
				TrackingMsg.msgReceived((TrackingMsg) msg);
				TrackingMsg tm = (TrackingMsg) msg;
				if (!trackingMessages.containsKey(tm.getTimestamp()))
					trackingMessages.put(tm.getTimestamp(),
							new ArrayList<TrackingMsg>());
				trackingMessages.get(tm.getTimestamp()).add(tm);
				if (trackingMessages.get(tm.getTimestamp()).size() >= 3) {
					Position[] pos = new Position[trackingMessages.get(
							tm.getTimestamp()).size()];
					Double[] dists = new Double[pos.length];
					int i = 0;
					for (TrackingMsg t_ : trackingMessages.get(tm
							.getTimestamp())) {
						pos[i] = t_.getNode().getEstimatedPosition();
						dists[i] = t_.getDistance();
						if (pos[i] == null || dists[i] == null) {
							pos[i] = null;
							dists[i] = null;
							continue;
						}
						i++;
					}
					if (i >= 3) {
						Position p = multilateration2d(pos, dists);
						double[] coords = filter.step(new double[] { p.xCoord,
								p.yCoord });
						CustomGlobal.target.setEstimatedPosition(p,
								new Position(coords[0], coords[1], 0.0), tm
										.getTimestamp());
					}
				}
				if (trackingMessages.size() > 1000) {
					trackingMessages.clear();
				}
			} else {
				Message m = msg.clone();
				if (this.routingTable.containsKey(CustomGlobal.sink)) {
					MessageTimer mt = new MessageTimer(m, CustomGlobal.sink);
					mt.startRelative(1, this);
				}
			}

		}
	}

	public String showPath(LocalizableNode src, LocalizableNode dst) {
		String result = null;
		if (src != null && dst != null && src.ID != dst.ID) {
			result = "AODV:";
			LocalizableNode aux = src;
			while (true) {
				if (aux == dst) {
					result += aux.ID;
					break;
				}
				result += aux.ID
						+ (aux.routingTable.get(dst) == null ? "" : "("
								+ aux.routingTable.get(dst).hops + ")") + "->";
				if (aux.routingTable.containsKey(dst))
					aux = (LocalizableNode) aux.routingTable.get(dst).next;
				else {
					result = null;
					break;
				}

			}
		}
		return result;
	}

	@Override
	public void handleMessages(Inbox inbox) {
		for (Message m : inbox) {
			Double d = estimateDistance(inbox.getSender());
			neighbors.put(inbox.getSender(), d);
			handleMessage(m, inbox.getSender());
			// if (((LocalizableNode) inbox.getSender()).getEstimatedPosition()
			// != null) {
			if (m instanceof LocalizationMessage && this.isFreeNode()) {
				localizedNeighbors.put(inbox.getSender(), d);
				if (CustomGlobal.getLocalizationAlgorithm() == null) {
				} else if (CustomGlobal.getLocalizationAlgorithm()
						.equals("RPE")) {
					handleRPE();
				} else if (CustomGlobal.getLocalizationAlgorithm()
						.equals("DPE")) {
					handleDPE();
				}
			}
		}
	}

	protected void handleRPE() {
		if (this.isFreeNode()
				&& localizedNeighbors.size() >= CustomGlobal.minReferences()) {
			try {
				LocalizableNode[] references = new LocalizableNode[localizedNeighbors
						.size()];
				Double[] distances = new Double[references.length];
				int i = 0;
				for (Node nd : localizedNeighbors.keySet()) {
					distances[i] = localizedNeighbors.get(nd);
					references[i++] = (LocalizableNode) nd;
				}
				// Só ordena se precisar
				if (references.length > CustomGlobal.maxReferences())
					Arrays.sort(references, new ResidualComparator());
				for (i = 0; i < references.length; i++) {
					distances[i] = localizedNeighbors.get(references[i]);
				}
				this
						.setEstimatedPosition(Configuration.dimensions == 2 ? multilateration2d(
								references, distances)
								: multilateration3d(references, distances));
				calculateResidual(references, distances);
				if (this.getResidual() < CustomGlobal.maxResidual) {
					MessageTimer mt = new MessageTimer(new LocalizationMessage(
							this, 0));
					mt.startRelative(1, this);
				}
			} catch (Exception e) {
				System.out.println("HandleMessage Error! " + e.getMessage());
			}
		}
	}

	protected void handleDPE() {
		if (this.isFreeNode()
				&& localizedNeighbors.size() >= CustomGlobal.minReferences()) {
			try {
				// Pegando todas as mensagens
				LocalizableNode[] references = new LocalizableNode[localizedNeighbors
						.size()];
				Double[] distances = new Double[references.length];
				int i = 0;
				for (Node nd : localizedNeighbors.keySet()) {
					distances[i] = localizedNeighbors.get(nd);
					references[i++] = (LocalizableNode) nd;
				}

				// Ordenando os índices dos pares de referências com o seguinte
				// critério: mais distantes entre si e mais próximas da origem
				// da recursão
				Vector<Integer> references1 = new Vector<Integer>();
				Vector<Integer> references2 = new Vector<Integer>();
				double choice = -1;
				for (int k = 0; k < references.length; k++) {
					for (int z = 0; z < references.length; z++) {
						if (z != k) {
							double db = references[k].getEstimatedPosition()
									.distanceTo(
											references[z]
													.getEstimatedPosition());
							double dc = references[k].getEstimatedPosition()
									.distanceTo(
											references[k].getRecursionOrigin())
									+ references[z]
											.getEstimatedPosition()
											.distanceTo(
													references[z]
															.getRecursionOrigin());
							double tmp = db / dc;
							if (tmp > choice) {
								choice = tmp;
								references1.insertElementAt(z, 0);
								references2.insertElementAt(k, 0);
							}
						}
					}
				}

				// Caso nao seja possível calcular a interseção entre as
				// melhores referências passar para as próximas melhores
				int ir1 = -1;
				int ir2 = -1;
				int k = 0;
				Position[] points = null;
				do {
					if (k >= references1.size()) {
						break;
					} else {
						ir1 = references1.elementAt(k);
						ir2 = references2.elementAt(k);
						points = circleIntersections(references[ir1],
								distances[ir1], references[ir2], distances[ir2]);
						k++;
					}
				} while (points == null);

				// Escolhendo o ponto mais distante da origem da recursão
				/*
				 * if ( points[0].distanceTo(
				 * references[ir1].getRecursionOrigin() ) >
				 * points[1].distanceTo( references[ir1].getRecursionOrigin() )
				 * ) this.setEstimatedPosition(points[0]); else
				 * this.setEstimatedPosition(points[1]);
				 * this.setRecursionOrigin( references[ir1].getRecursionOrigin()
				 * );
				 */
				// Evitando a solução errada
				if (points != null && points.length > 1) {
					if (points[0].distanceTo(this.getPosition()) < points[1]
							.distanceTo(this.getPosition()))
						this.setEstimatedPosition(points[0]);
					else
						this.setEstimatedPosition(points[1]);
					this.setRecursionOrigin(references[ir1]
							.getRecursionOrigin());

					// Calculando o residual
					calculateResidual(references, distances);
					// Na verdade o DPE nao bloqueia nada com base no residuo
					// MessageTimer mt = new MessageTimer(new
					// LocalizationMessage(
					// this, 0));
					// mt.startRelative(1, this);
					// onLocalized();
					MessageTimer mt = new MessageTimer(new LocalizationMessage(
							this, 0));
					mt.startRelative(1, this);
				}
			} catch (Exception e) {
				System.out.println("HandleMessage Error! " + e.getMessage());
				e.printStackTrace();
			}
		}

	}

	@Override
	public void init() {
		// TODO Auto-generated method stub

	}

	@Override
	public void neighborhoodChange() {
		// TODO Auto-generated method stub

	}

	@Override
	public void postStep() {

	}

	@Override
	public void sleep() {
		// TODO Auto-generated method stub

	}

	@Override
	public void wakeUp() {
		// TODO Auto-generated method stub

	}

	public Position getRecursionOrigin() {
		return recursionOrigin;
	}

	public void setRecursionOrigin(Position recursionOrigin) {
		this.recursionOrigin = recursionOrigin;
	}

	protected Position[] circleIntersections(LocalizableNode reference1,
			double distance1, LocalizableNode reference2, double distance2) {
		if (reference1.getEstimatedPosition() == null
				|| reference2.getEstimatedPosition() == null)
			return null;
		double x0 = reference1.getEstimatedPosition().xCoord;
		double y0 = reference1.getEstimatedPosition().yCoord;
		double r0 = distance1;

		double x1 = reference2.getEstimatedPosition().xCoord;
		double y1 = reference2.getEstimatedPosition().yCoord;
		double r1 = distance2;

		double a, dx, dy, d, h, rx, ry;
		double x2, y2;

		dx = x1 - x0;
		dy = y1 - y0;
		d = Math.hypot(dx, dy);
		if ((d > (r0 + r1)) || (d < Math.abs(r0 - r1)))
			return null;
		a = ((r0 * r0) - (r1 * r1) + (d * d)) / (2.0 * d);
		x2 = x0 + (dx * a / d);
		y2 = y0 + (dy * a / d);
		h = Math.sqrt((r0 * r0) - (a * a));
		rx = -dy * (h / d);
		ry = dx * (h / d);

		Position[] points = new Position[2];
		points[0] = new Position(x2 + rx, y2 + ry, 0);
		points[1] = new Position(x2 - rx, y2 - ry, 0);
		return points;
	}

	public void draw(Graphics g, PositionTransformation pt, boolean highlight) {
		if (!f) {
			this.defaultDrawingSizeInPixels /= pt.getZoomFactor();
			f = true;
		}
		if (this.sink) {
			this.setColor(Color.pink);
		} else if (this.getState() == NodeState.WakingUp) {
			this.setColor(Color.cyan);
		} else if (this.isBeaconNode())
			this.setColor(Color.green);
		else if (this.isSettledNode())
			this.setColor(Color.blue);
		if (this.isFreeNode())
			this.setColor(Color.gray);
		if (this.getPosition().distanceTo(CustomGlobal.target.getPosition()) <= CustomGlobal.sensorRange
				&& CustomGlobal.targetEnabled) {
			highlight = true;
		} else
			highlight = false;
		super.draw(g, pt, highlight);
	}

	private class ResidualComparator implements Comparator<LocalizableNode> {
		public int compare(LocalizableNode node1, LocalizableNode node2) {
			if (node1.getResidual() == node2.getResidual())
				return 0;
			else if (node1.getResidual() > node2.getResidual())
				return 1;
			else
				return -1;
		}
	}

	public boolean isSink() {
		return sink;
	}

	public void setSink(boolean sink) {
		this.sink = sink;
		this.useRealPosition();
	}

	public String toString() {
		return "" + ID + "; residual=" + getResidual();
	}

	public double estimatedDistanceTo(LocalizableNode node) {
		if (node.getEstimatedPosition() == null
				|| this.getEstimatedPosition() == null)
			return Double.NaN;
		else
			return node.getEstimatedPosition().distanceTo(
					this.getEstimatedPosition());
	}

	public double distanceTo(LocalizableNode node) {
		return node.getPosition().distanceTo(this.getPosition());
	}

	public void afterWait(Message m) {

	}
}
